#ifndef ROBOT_MINI_DOG
#define ROBOT_MINI_DOG

#include "utils.h"
#include "modular.h"

#define MINI_DOG_LEN 8 // 自由度
#define MINI_DOG_PI 3.14159f
#define MINI_DOG_ANGLE_TO_DIGITAL 3.41f
#define MINI_DOG_TS 1 // 步态周期计算量
#define MINI_DOG_FAI 0.5 	//支撑向占空比
#define MINI_DOG_OFFSET 310        // 偏移量，计算方式：90/300*1023=306.9

// 初始化状态枚举
typedef enum{
	MINI_DOG_INIT_FALSE = 0,
	MINI_DOG_INIT_TRUE = !MINI_DOG_INIT_FALSE
}Mini_Dog_Init_Enum;

// 运动模式枚举
typedef enum{
	MINI_DOG_FORWARD = 0,
	MINI_DOG_BACK,
	MINI_DOG_STEP,
	MINI_DOG_RIGHT,
	MINI_DOG_LEFT,
	MINI_DOG_STOP,
}Mini_Dog_Motion_Enum;

// 步态模式枚举
typedef enum{
	MINI_DOG_TROT = 0,
	MINI_DOG_WALK,
	MINI_DOG_PRONK,
	MINI_DOG_BOUND
}Mini_Dog_Gait_Enum;

// 控制模式枚举
typedef enum{
	MINI_DOG_REMOTE_CONTROL = 0,
	MINI_DOG_CALIBRATION,
	MINI_DOG_ACTION,
	MINI_DOG_STABLE
}Mini_Dog_Mode_Enum;

// 姿态控制值结构体
typedef struct {
	float roll;
	float pitch;
	float x;
	float z;
} Mini_Dog_Attitude_Struct;

// PID结构体
typedef struct{
	float kp,kd,ki;
	float error;
	float last_error;
	float output;
	float diff;
	float integral;
} Mini_Dog_Pid_Struct;

//足端坐标结构体
typedef struct {
	float x[4];
	float z[4];
} Mini_Dog_Foot_Coordinate_Struct;

//标定数据结构体
typedef struct{
	uint8_t servo_id;
	uint32_t servo_position;
}Mini_Dog_Coordinate_Struct;

typedef struct{
	Mini_Dog_Init_Enum Status; // 初始化状态
	Mini_Dog_Mode_Enum Mode; // 控制模式
	Mini_Dog_Motion_Enum Motion_Mode; // 运动模式
	Mini_Dog_Gait_Enum Gait_Mode; // 步态模式
	float Gait_Time; // 步态时序
	uint32_t Gait_Cycle; // 步态周期
	Mini_Dog_Pid_Struct Pitch_Pid; // 俯仰角PID
	Mini_Dog_Pid_Struct Roll_Pid; // 滚转角PID
	Mini_Dog_Attitude_Struct Target; // 目标姿态值
	Mini_Dog_Attitude_Struct Current; // 当前姿态值
	uint16_t Zero_Position[MINI_DOG_LEN]; // 关节零位数据
	Mini_Dog_Coordinate_Struct Coordinate_Position; // 关节标定数据
	Mini_Dog_Foot_Coordinate_Struct Foot_Coordinate; //足端坐标系
	float Body_Length; // 身体长度
	float Body_Width; // 身体宽度
	float Foot_Width; // 足宽度
	float Thigh_Length; // 大腿长度
	float Calf_Length; // 小腿长度
	float Thigh[4]; // 大腿
	float Calf[4]; // 小腿
}Mini_Dog_Struct;

extern Mini_Dog_Struct Mini_Dog_Data;

void Mini_Dog_Init(void);

void Mini_Dog_Get_Calibration_Data(void);

float Mini_Dog_Stable_Calculation(float mpu_val, float target_val, Mini_Dog_Pid_Struct *pid);

float Mini_Dog_Pid_Calculation(float target_val, float current_val);

void Mini_Dog_Attitude_To_Coordinate(const Mini_Dog_Attitude_Struct *attitude, float x[], float z[]);

void Mini_Dog_Coordinate_To_Angle(const float x[], const float z[], float thigh[], float calf[]);

void Mini_Dog_Data_To_Servo(float thigh[], float calf[], uint16_t zero_position[], uint16_t speed);

void Mini_Dog_Gait_Generate_Time(float *gait_time, float it, float n);

void Mini_Dog_Trot_Gait_Generate(float gait_time, float step_width, float lift_height,  float x[], float z[]);

void Mini_Dog_Walk_Gait_Generate(float gait_time, float step_width, float lift_height,  float x[], float z[]);

void Mini_Dog_Trot_Pronk_Generate(float gait_time, Mini_Dog_Motion_Enum motion, float x[], float z[]);

void Mini_Dog_Remote_Control(void);

void Mini_Dog_Calibration(uint32_t id, uint32_t position);

void Mini_Dog_Action(void);

void Mini_Dog_Attitude_Interface(char *value, float current_value, float arget_value, uint16_t time);

void Mini_Dog_Task(void);

void Mini_Dog_Usb_Callback(char *type, int mode, int motion, int cycle, int id, int position);

#endif
